package mathlib;

import geometry.angle.Angle;
import geometry.angle.Angle.AngleType;
import geometry.angle.Angle.TrigFunction;

import java.text.DecimalFormat;

import matrix.Matrix;
import space.CelestialBody;
import space.LambertSolver;
import space.LambertSolver.LambertSolution;
import space.Orbit;
import space.Orbit.OrbitBuilder;
import space.Orbit.OrbitBuilder.OrbitDefinitionType;
import space.OrbitPosition;
import vector.Vector;
import vector.Vector3D;

public class OrbitCalc {

	public static void old(String args[]) {

		// Constants
		final double AU = 149597871.0; // km in an AU
		final double YEAR_S = 1.0 / (60 * 60 * 24 * 365); // years in a second
		final double R_JUP = 71370.0;
		DecimalFormat decForm = new DecimalFormat("#.000");

		// Establish Celestial Bodies
		CelestialBody earth = new CelestialBody("Earth", 5.9726E24);
		CelestialBody sun = new CelestialBody("Sun", 1988500E24);
		CelestialBody jupiter = new CelestialBody("Jupiter", 1.8981E27);

		// Build the LEO parking orbit
		OrbitBuilder builder = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		builder.setCentralBody(earth);
		builder.setSMA(6378.0 * 1.1);
		builder.setEcc(0);
		builder.setInc(new Angle(0));
		builder.setRightAscOfAscNode(new Angle(0));
		builder.setArgOfPeri(new Angle(0));
		Orbit parkingOrbit = builder.build();

		// Build earth's orbit around the sun
		OrbitBuilder build = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(1.0 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit earthsOrbit = build.build();

		// Build Jupiter's Orbit around the Sun
		build.setCentralBody(sun);
		build.setSMA(5.2 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit jupitersOrbit = build.build();

		// Build the Earth - Jupiter heliocentric Hohmann transfer orbit
		OrbitPosition helioHohmStart = new OrbitPosition(earthsOrbit,
				new Angle(0));
		OrbitPosition helioHohmEnd = new OrbitPosition(jupitersOrbit,
				new Angle(Math.PI));
		LambertSolver solver = new LambertSolver(helioHohmStart, helioHohmEnd);
		LambertSolution solution = solver.solution(solver.aMin(), true);
		Orbit helioHohm = solution.orbitalPath();
		Vector3D hohmVp = helioHohm.velocityVectorAt(new Angle(0));
		Vector3D hohmVa = helioHohm.velocityVectorAt(new Angle(Math.PI));

		// System.out.println(hohmVp.magnitude());
		// System.out.println(hohmVa.magnitude());

		// Calculate the hyperbolic orbit around the Earth that will result in
		// this transfer
		Vector3D earthVelAtStart = earthsOrbit.velocityVectorAt(new Angle(0));
		Vector3D earthVInfVec = (Vector3D) hohmVp.add(earthVelAtStart
				.scalarMultiply(-1.0));
		double earthVInf = earthVInfVec.magnitude();
		double parkingRadius = parkingOrbit.radiusAt(new Angle(0));
		double parkingVel = parkingOrbit.velocityAt(new Angle(0));
		double sinDeltaBy2Earth = 1.0 / (1.0 + (parkingRadius * earthVInf * earthVInf)
				/ earth.gravParam());
		Angle deltaEarth = new Angle(sinDeltaBy2Earth, TrigFunction.SINE)
				.scalarMultiply(2.0);
		double earthEnergy = 0.5 * earthVInf * earthVInf;
		double earthPeriVel = Math.sqrt(2 * (earthEnergy + earth.gravParam()
				/ parkingRadius));
		double earthDeltaV = earthPeriVel - parkingVel;

		System.out.println("EARTH DEPARTURE:");
		System.out.println("BURN MAGNITUDE: " + decForm.format(earthDeltaV)
				+ " km/s");
		System.out.println("ANGLE BEFORE SUN/EARTH LINE: "
				+ deltaEarth.scalarMultiply(0.5).measure(AngleType.DEGREES)
				+ " DEGREES");

		// Calculate the first Jupiter flyby, which will put us on a 2:3
		// resonance with it
		double firstOrbitRes = 2.0 / 3.0; // Ratio of # orbits s/c makes to #
											// orbits jupiter makes
		double firstFlybyTargetPeriod = jupitersOrbit.period() / firstOrbitRes;
		double firstFlySMA = Math.pow(
				sun.gravParam()
						* Math.pow(firstFlybyTargetPeriod / (2.0 * Math.PI),
								2.0), 1.0 / 3.0);
		double jupSunRad = jupitersOrbit.radiusAt(new Angle(0));
		Vector3D jupVelVec = jupitersOrbit.velocityVectorAt(new Angle(Math.PI));
		double jupVel = jupVelVec.magnitude();
		double firstFlyEndVel = Math.sqrt(sun.gravParam()
				* (2.0 / jupSunRad - 1.0 / firstFlySMA));
		double firstFlyStartVel = hohmVa.magnitude();
		double firstFlyVInf = jupVel - firstFlyStartVel;
		double firstFlyCosDelta = ((firstFlyEndVel * firstFlyEndVel)
				- (firstFlyVInf * firstFlyVInf) - (jupVel * jupVel))
				/ (-2.0 * jupVel * firstFlyVInf);
		Angle firstFlyDelta = new Angle(firstFlyCosDelta, TrigFunction.COSINE);
		Vector3D firstVInfDir = (Vector3D) hohmVa.direction();
		Vector3D firstHDir = (Vector3D) helioHohm.getAngMomentum().direction();
		double firstSinDeltaBy2 = (firstFlyDelta.scalarMultiply(0.5)).sin();
		double firstRp = (jupiter.gravParam() / (firstFlyVInf * firstFlyVInf))
				* (1.0 / firstSinDeltaBy2 - 1.0);
		Vector3D firstFlybyFinalVelVec = (Vector3D) jupVelVec
				.add(firstVInfDir.scalarMultiply(firstFlyVInf
						* firstFlyDelta.cos())).add(
						firstHDir.scalarMultiply(firstFlyVInf
								* firstFlyDelta.sin()));
		OrbitPosition firstFlybyOrbit = new OrbitPosition(sun,
				jupitersOrbit.radiusVectorAt(new Angle(Math.PI)),
				firstFlybyFinalVelVec);

		System.out.println("");
		System.out.println("FIRST FLYBY:");
		System.out.println("FLYBY DISTANCE: " + (firstRp / R_JUP)
				+ " JUPITER RADII");
		System.out
				.println("FINAL VELOCITY: " + firstFlybyFinalVelVec + " KM/S");
		System.out.println("FINAL INCLINATION: "
				+ firstFlybyOrbit.getInc().measure(AngleType.DEGREES)
				+ " DEGREES");
		System.out.println("FINAL SEMI-MAJOR AXIS: "
				+ (firstFlybyOrbit.getSMA() / AU) + " AU");
		System.out
				.println("FINAL APHELION: "
						+ ((firstFlybyOrbit.getSMA() * (1 + firstFlybyOrbit
								.getEcc())) / AU) + " AU");
		System.out
				.println("FINAL RESONANCE: "
						+ (firstFlybyOrbit.getOrbit().period() / jupitersOrbit
								.period()));

		// Calculate the seond Jupiter flyby
		double secondOrbitRes = 2.0 / 3.0; // Ratio of # orbits s/c makes to #
											// orbits jupiter makes
		double secondFlybyTargetPeriod = jupitersOrbit.period()
				/ secondOrbitRes;
		double secondFlySMA = Math.pow(
				sun.gravParam()
						* Math.pow(secondFlybyTargetPeriod / (2.0 * Math.PI),
								2.0), 1.0 / 3.0);
		double secondFlyEndVel = Math.sqrt(sun.gravParam()
				* (2.0 / jupSunRad - 1.0 / secondFlySMA));
		double secondFlyVInf = jupVelVec.add(
				firstFlybyFinalVelVec.scalarMultiply(-1.0)).magnitude();
		double secondFlyCosDelta = ((secondFlyEndVel * secondFlyEndVel)
				- (secondFlyVInf * secondFlyVInf) - (jupVel * jupVel))
				/ (-2.0 * jupVel * secondFlyVInf);
		Angle secondFlyDelta = new Angle(secondFlyCosDelta, TrigFunction.COSINE);
		Vector3D secondVInfVec = (Vector3D) firstFlybyFinalVelVec.add(jupVelVec
				.scalarMultiply(-1.0));
		double secondSinDeltaBy2 = (secondFlyDelta.scalarMultiply(0.5)).sin();
		double secondRp = (jupiter.gravParam() / (secondFlyVInf * secondFlyVInf))
				* (1.0 / secondSinDeltaBy2 - 1.0);

		Angle gamma = secondVInfVec.angleTo(new Vector3D(0, -1, 0));
		Angle psi = (new Angle(Math.PI)).add(gamma.scalarMultiply(-1.0).add(
				secondFlyDelta.scalarMultiply(-1.0)));

		Vector3D secondFlybyVInfOut = (Vector3D) new Vector3D(0, psi.cos(),
				-1.0 * psi.sin()).scalarMultiply(secondFlyVInf);
		// Vector3D secondFlybyVInfOut = (Vector3D)
		// secondVInfDir.scalarMultiply(-1.0 * secondFlyVInf *
		// secondFlyDelta.cos()).add(secondHDir.scalarMultiply(secondFlyVInf *
		// secondFlyDelta.sin()));
		Vector3D secondFlybyFinalVelVec = (Vector3D) jupVelVec
				.add(secondFlybyVInfOut);
		OrbitPosition secondFlybyOrbit = new OrbitPosition(sun,
				jupitersOrbit.radiusVectorAt(new Angle(Math.PI)),
				secondFlybyFinalVelVec);

		System.out.println("");
		System.out.println("SECOND FLYBY:");
		System.out.println("FLYBY DISTANCE: " + (secondRp / R_JUP)
				+ " JUPITER RADII");
		System.out.println("FINAL VELOCITY: " + secondFlybyFinalVelVec
				+ " KM/S");
		System.out.println("FINAL INCLINATION: "
				+ secondFlybyOrbit.getInc().measure(AngleType.DEGREES)
				+ " DEGREES");
		System.out.println("FINAL SEMI-MAJOR AXIS: "
				+ (secondFlybyOrbit.getSMA() / AU) + " AU");
		System.out
				.println("FINAL APHELION: "
						+ ((secondFlybyOrbit.getSMA() * (1 + secondFlybyOrbit
								.getEcc())) / AU) + " AU");
		System.out.println("FINAL RESONANCE: "
				+ (secondFlybyOrbit.getOrbit().period() / jupitersOrbit
						.period()));
		System.out.println(jupitersOrbit.radiusVectorAt(new Angle(Math.PI)));
	}

	
	public static void legacy(String args[]) {

		// Constants
		final double AU = 149597871.0; // km in an AU
		final double YEAR_S = 1.0 / (60 * 60 * 24 * 365); // years in a second
		final double R_JUP = 71370.0;
		DecimalFormat decForm = new DecimalFormat("#.000");

		// Establish Celestial Bodies
		CelestialBody earth = new CelestialBody("Earth", 5.9726E24);
		CelestialBody sun = new CelestialBody("Sun", 1988500E24);
		CelestialBody jupiter = new CelestialBody("Jupiter", 1.8981E27);

		// Build the LEO parking orbit
		OrbitBuilder builder = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		builder.setCentralBody(earth);
		builder.setSMA(6378.0 * 1.1);
		builder.setEcc(0);
		builder.setInc(new Angle(0));
		builder.setRightAscOfAscNode(new Angle(0));
		builder.setArgOfPeri(new Angle(0));
		Orbit parkingOrbit = builder.build();

		// Build earth's orbit around the sun
		OrbitBuilder build = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(1.0 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit earthsOrbit = build.build();

		// Build Jupiter's Orbit around the Sun
		build.setCentralBody(sun);
		build.setSMA(5.2 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit jupitersOrbit = build.build();

		// Build the Earth - Jupiter heliocentric Hohmann transfer orbit
		OrbitPosition helioHohmStart = new OrbitPosition(earthsOrbit,
				new Angle(0));
		OrbitPosition helioHohmEnd = new OrbitPosition(jupitersOrbit,
				new Angle(Math.PI));
		LambertSolver solver = new LambertSolver(helioHohmStart, helioHohmEnd);
		LambertSolution solution = solver.solution(solver.aMin(), true);
		Orbit helioHohm = solution.orbitalPath();
		Vector3D hohmVp = helioHohm.velocityVectorAt(new Angle(0));
		Vector3D hohmVa = helioHohm.velocityVectorAt(new Angle(Math.PI));

		// Calculate the hyperbolic orbit around the Earth that will result in
		// this transfer
		Vector3D earthVelAtStart = earthsOrbit.velocityVectorAt(new Angle(0));
		Vector3D earthVInfVec = (Vector3D) hohmVp.add(earthVelAtStart
				.scalarMultiply(-1.0));
		double earthVInf = earthVInfVec.magnitude();
		double parkingRadius = parkingOrbit.radiusAt(new Angle(0));
		double parkingVel = parkingOrbit.velocityAt(new Angle(0));
		double sinDeltaBy2Earth = 1.0 / (1.0 + (parkingRadius * earthVInf * earthVInf)
				/ earth.gravParam());
		Angle deltaEarth = new Angle(sinDeltaBy2Earth, TrigFunction.SINE)
				.scalarMultiply(2.0);
		double earthEnergy = 0.5 * earthVInf * earthVInf;
		double earthPeriVel = Math.sqrt(2 * (earthEnergy + earth.gravParam()
				/ parkingRadius));
		double earthDeltaV = earthPeriVel - parkingVel;

		System.out.println("EARTH DEPARTURE:");
		System.out.println("BURN MAGNITUDE: " + decForm.format(earthDeltaV)
				+ " km/s");
		System.out.println("ANGLE BEFORE SUN/EARTH LINE: "
				+ deltaEarth.scalarMultiply(0.5).measure(AngleType.DEGREES)
				+ " DEGREES");

		// Calculate the Jupiter encounter
		Vector3D jupVelVector = jupitersOrbit.velocityVectorAt(new Angle(
				Math.PI));
		Vector3D jupPosVector = jupitersOrbit
				.radiusVectorAt(new Angle(Math.PI));
		Vector3D vInfIn = (Vector3D) jupVelVector.add(hohmVa
				.scalarMultiply(-1.0));
		double energyIn = 0.5 * vInfIn.magnitude() * vInfIn.magnitude();
		double rp = 7.06758 * R_JUP;
//		double rp = 20.893 * R_JUP;
		double sinDeltaBy2 = 1.0 / (1.0 + (rp * vInfIn.magnitude() * vInfIn
				.magnitude()) / jupiter.gravParam());
		Angle deltaIn = new Angle(sinDeltaBy2, TrigFunction.SINE)
				.scalarMultiply(2.0);
		Vector3D rpDir = new Vector3D(0, deltaIn.scalarMultiply(0.5).sin(),
				-1.0 * deltaIn.scalarMultiply(0.5).cos());
		Vector3D rpVec = (Vector3D) rpDir.scalarMultiply(rp);
		Vector3D vpDir = new Vector3D(0, deltaIn.scalarMultiply(0.5).cos(),
				deltaIn.scalarMultiply(0.5).sin());
		double vp = Math.sqrt(2.0 * (energyIn + jupiter.gravParam() / rp));
		Vector3D vpVec = (Vector3D) vpDir.scalarMultiply(vp);

		Vector3D deltaV = (Vector3D) vpDir.direction().scalarMultiply(0)
				.add(new Vector3D(0, 0, 0));

		vpVec = (Vector3D) vpVec.add(deltaV);
		double energyOut = 0.5 * vpVec.magnitude() * vpVec.magnitude()
				- jupiter.gravParam() / rp;
		double vInfOutMag = Math.sqrt(2.0 * energyOut);
		double sinDeltaBy2Out = 1.0 / (1.0 + (rp * vInfOutMag * vInfOutMag)
				/ jupiter.gravParam());
		Angle deltaOut = new Angle(sinDeltaBy2Out, TrigFunction.SINE)
				.scalarMultiply(2.0);
		Angle gamma = vpDir.angleTo(new Vector3D(0, 1, 0));
		Angle psi = gamma.add(deltaOut.scalarMultiply(0.5));
		Vector3D vInfOutDir = new Vector3D(0, -1.0 * psi.cos(), psi.sin());
		Vector3D vInfOut = (Vector3D) vInfOutDir.scalarMultiply(vInfOutMag);

		Vector3D finalVel = (Vector3D) vInfOut.add(jupVelVector);
		Orbit finalOrbit = new Orbit(sun, jupPosVector, finalVel);

		// double minVel = finalOrbit.velocityAt(new Angle(Math.PI));
		// Angle leftToTarget = new Angle(90,
		// AngleType.DEGREES).add(finalOrbit.getInc().scalarMultiply(-1.0));
		// double deltaVToTarget = 2.0 * minVel *
		// leftToTarget.scalarMultiply(0.5).sin();

		System.out.println("");
		System.out.println("AFTER FLYBY AND BURN:");
		System.out
				.println("FLYBY DISTANCE: " + (rp / R_JUP) + " JUPITER RADII");
		System.out.println("DELTA V EXPENDED: " + deltaV.magnitude() + " KM/S");
		System.out.println("FINAL VELOCITY: " + finalVel + " KM/S");
		System.out.println("FINAL INCLINATION: "
				+ finalOrbit.getInc().measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("FINAL SEMI-MAJOR AXIS: "
				+ (finalOrbit.getSMA() / AU) + " AU");
		System.out.println("FINAL APHELION: "
				+ ((finalOrbit.getSMA() * (1 + finalOrbit.getEcc())) / AU)
				+ " AU");
		System.out.println("FINAL PERIHELION: "
				+ ((finalOrbit.getSMA() * (1 - finalOrbit.getEcc())) / AU)
				+ " AU");
		System.out.println("FINAL RESONANCE: "
				+ (finalOrbit.period() / jupitersOrbit.period()));
	}

	
	public static void latestOld(String args[]) {
		// Constants
		final double AU = 149597871.0; // km in an AU
		final double YEAR_S = 1.0 / (60 * 60 * 24 * 365); // years in a second
		final double R_JUP = 71370.0;
		DecimalFormat decForm = new DecimalFormat("#.000");

		// Establish Celestial Bodies
		CelestialBody earth = new CelestialBody("Earth", 5.9726E24);
		CelestialBody sun = new CelestialBody("Sun", 1988500E24);
		CelestialBody jupiter = new CelestialBody("Jupiter", 1.8981E27);

		// Build the LEO parking orbit
		OrbitBuilder builder = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		builder.setCentralBody(earth);
		builder.setSMA(6378.0 * 1.1);
		builder.setEcc(0);
		builder.setInc(new Angle(0));
		builder.setRightAscOfAscNode(new Angle(0));
		builder.setArgOfPeri(new Angle(0));
		Orbit parkingOrbit = builder.build();

		// Build earth's orbit around the sun
		OrbitBuilder build = new OrbitBuilder(
				OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(1.0 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit earthsOrbit = build.build();

		// Build Jupiter's Orbit around the Sun
		build.setCentralBody(sun);
		build.setSMA(5.2 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit jupitersOrbit = build.build();

		// Build the Earth - Jupiter heliocentric Hohmann transfer orbit
		OrbitPosition helioHohmStart = new OrbitPosition(earthsOrbit,
				new Angle(0));
		OrbitPosition helioHohmEnd = new OrbitPosition(jupitersOrbit,
				new Angle(Math.PI));
		LambertSolver solver = new LambertSolver(helioHohmStart, helioHohmEnd);
		LambertSolution solution = solver.solution(solver.aMin(), true);
		Orbit helioHohm = solution.orbitalPath();
		Vector3D hohmVp = helioHohm.velocityVectorAt(new Angle(0));
		Vector3D hohmVa = helioHohm.velocityVectorAt(new Angle(Math.PI));

		// Calculate the hyperbolic orbit around the Earth that will result in
		// this transfer
		Vector3D earthVelAtStart = earthsOrbit.velocityVectorAt(new Angle(0));
		Vector3D earthVInfVec = (Vector3D) hohmVp.add(earthVelAtStart
				.scalarMultiply(-1.0));
		double earthVInf = earthVInfVec.magnitude();
		double parkingRadius = parkingOrbit.radiusAt(new Angle(0));
		double parkingVel = parkingOrbit.velocityAt(new Angle(0));
		double sinDeltaBy2Earth = 1.0 / (1.0 + (parkingRadius * earthVInf * earthVInf)
				/ earth.gravParam());
		Angle deltaEarth = new Angle(sinDeltaBy2Earth, TrigFunction.SINE)
				.scalarMultiply(2.0);
		double earthEnergy = 0.5 * earthVInf * earthVInf;
		double earthPeriVel = Math.sqrt(2 * (earthEnergy + earth.gravParam()
				/ parkingRadius));
		double earthDeltaV = earthPeriVel - parkingVel;

		System.out.println("EARTH DEPARTURE:");
		System.out.println("BURN MAGNITUDE: " + decForm.format(earthDeltaV)
				+ " km/s");
		System.out.println("ANGLE BEFORE SUN/EARTH LINE: "
				+ deltaEarth.scalarMultiply(0.5).measure(AngleType.DEGREES)
				+ " DEGREES");
		
		// Some useful constants
		Vector3D jupPosVec = jupitersOrbit.radiusVectorAt(new Angle(Math.PI));
		double jupPos = jupPosVec.magnitude();
		Vector3D jupVelVec = jupitersOrbit.velocityVectorAt(new Angle(Math.PI));
		double jupVel = jupVelVec.magnitude();
		
		// Calculate the first flyby around Jupiter - to go into a 2:3 resonance
		double res1 = 3.0 / 2.0;
		double period1 = jupitersOrbit.period() / res1;
		double sma1 = Math.pow((sun.gravParam() * period1 * period1) / (4 * Math.PI * Math.PI), 1.0 / 3.0);
		double vOut1 = Math.sqrt(sun.gravParam() * (2.0 / jupPos - 1.0 / sma1));
		Vector3D vInfIn1 = (Vector3D) jupVelVec.add(hohmVa.scalarMultiply(-1.0));
		double cosDelta1 = ((vOut1 * vOut1) - (jupVel * jupVel) - (vInfIn1.magnitude() * vInfIn1.magnitude()))
				/ (-2.0 * jupVel * vInfIn1.magnitude());
		Angle delta1 = new Angle(cosDelta1, TrigFunction.COSINE);
		double rp1 = (jupiter.gravParam() / (vInfIn1.magnitude() * vInfIn1.magnitude())) * (1.0 / delta1.scalarMultiply(0.5).sin() - 1);
//		double rp1 = 5.0 * R_JUP;
//		double progradeDeltaVFlyby = 0.25;
		double progradeDeltaVFlyby = 0.0;
		Vector3D vFinal1 = velAfterFlybyVec(jupiter, jupVelVec, rp1, true, hohmVa, progradeDeltaVFlyby);
		double finalEnergy1 = 0.5 * vFinal1.magnitude() * vFinal1.magnitude() - sun.gravParam() / jupPos;
		double finalSMA1 = sun.gravParam() / (-2.0 * finalEnergy1);
		double finalPeriod = 2.0 * Math.PI * Math.sqrt(Math.pow(finalSMA1, 3.0) / sun.gravParam());
		
		Orbit flybyFinalOrbit1 = new Orbit(sun, jupPosVec, vFinal1);
		finalPeriod = flybyFinalOrbit1.period();
		Vector3D finalAphelionVel = flybyFinalOrbit1.velocityVectorAt(new Angle(Math.PI));
		Angle leftToPolar = new Angle(90, AngleType.DEGREES).add(flybyFinalOrbit1.getInc().scalarMultiply(-1.0));
		double deltaVToPolar = 2 * finalAphelionVel.magnitude() * leftToPolar.scalarMultiply(0.5).sin();
		
		System.out.println("");
		System.out.println("AFTER FIRST FLYBY: ");
		System.out.println("FLYBY RADIUS: " + (rp1 / R_JUP) + " JUPITER RADII");
		System.out.println("DELTA V DURING FLYBY: " + progradeDeltaVFlyby + " KM/S");
		System.out.println("TURN ANGLE DURING FLYBY: " + delta1.measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("FINAL VELOCITY: " + vFinal1 + "KM/S");
		System.out.println("FINAL INCLINATION: " + flybyFinalOrbit1.getInc().measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("FINAL RESONANCE: " + jupitersOrbit.period() / finalPeriod);
		System.out.println("FINAL SMA: " + flybyFinalOrbit1.getSMA() / AU + " AU");
		System.out.println("FINAL APHELION: " + ((flybyFinalOrbit1.getSMA() * (1 + flybyFinalOrbit1.getEcc())) / AU) + " AU");
		System.out.println("FINAL PERIHELION: " + ((flybyFinalOrbit1.getSMA() * (1 - flybyFinalOrbit1.getEcc())) / AU) + " AU");
		System.out.println("FINAL PERIOD: " + flybyFinalOrbit1.period() * YEAR_S + " YEARS");
		System.out.println("FINAL APHELION VELOCITY: " + finalAphelionVel + " KM/S");
		System.out.println("DELTA V TO POLAR: " + deltaVToPolar + " KM/S");
		System.out.println("RADIUS AT SUN'S POLES: " + (flybyFinalOrbit1.radiusAt(new Angle(Math.PI / 2.0)) / AU) + " AU");
		
		
		
		
		// Calculate the second flyby around Jupiter - to go into a 3:2 resonance
//		double res2 = 3.0 / 2.0;
//		double period2 = jupitersOrbit.period() / res2;
//		double sma2 = Math.pow((sun.gravParam() * period2 * period2) / (4 * Math.PI * Math.PI), 1.0 / 3.0);
//		double vOut2 = Math.sqrt(sun.gravParam() * (2.0 / jupPos - 1.0 / sma2));
//		Vector3D vInfIn2 = (Vector3D) jupVelVec.add(vFinal1.scalarMultiply(-1.0));
//		double cosDelta2 = ((vOut2 * vOut2) - (jupVel * jupVel) - (vInfIn2.magnitude() * vInfIn2.magnitude()))
//				/ (-2.0 * jupVel * vInfIn2.magnitude());
//		Angle delta2 = new Angle(cosDelta2, TrigFunction.COSINE);
//		double rp2 = (jupiter.gravParam() / (vInfIn2.magnitude() * vInfIn2.magnitude())) * (1.0 / delta2.scalarMultiply(0.5).sin() - 1);
//		Vector3D vFinal2 = velAfterFlybyVec(jupiter, jupVelVec, rp2, true, vFinal1);
//		double finalEnergy2 = 0.5 * vFinal2.magnitude() * vFinal2.magnitude() - sun.gravParam() / jupPos;
//		double finalSMA2 = sun.gravParam() / (-2.0 * finalEnergy2);
//		double finalPeriod2 = 2.0 * Math.PI * Math.sqrt(Math.pow(finalSMA2, 3.0) / sun.gravParam());
//		
//		Orbit flybyFinalOrbit2 = new Orbit(sun, jupPosVec, vFinal2);
//		finalPeriod2 = flybyFinalOrbit2.period();
//		
//		System.out.println("");
//		System.out.println("AFTER SECOND FLYBY: ");
//		System.out.println("FLYBY RADIUS: " + (rp2 / R_JUP) + " JUPITER RADII");
//		System.out.println("FINAL VELOCITY: " + vFinal2 + "KM/S");
//		System.out.println("FINAL INCLINATION: " + flybyFinalOrbit2.getInc().measure(AngleType.DEGREES) + " DEGREES");
//		System.out.println("FINAL RESONANCE: " + jupitersOrbit.period() / finalPeriod2);
//		System.out.println(vOut2);
//		System.out.println(vFinal1);
//		System.out.println(vFinal2);
//		
//		double sinDeltaMaxBy2 = 1.0 / (1.0 + (vInfIn1.magnitude() * vInfIn1.magnitude() * 5 * R_JUP) / jupiter.gravParam());
//		Angle deltaMax = new Angle(sinDeltaMaxBy2, TrigFunction.SINE).scalarMultiply(2.0);
//		System.out.println(deltaMax.measure(AngleType.DEGREES));
	}

	
	public static void main(String args[]) {
		// Constants
		final double AU = 149597871.0; // km in an AU
		final double YEAR_S = 1.0 / (60 * 60 * 24 * 365); // years in a second
		final double R_JUP = 71370.0;
		DecimalFormat decForm = new DecimalFormat("#.00");
		
		// Establish Celestial Bodies
		CelestialBody earth = new CelestialBody("Earth", 5.9726E24);
		CelestialBody sun = new CelestialBody("Sun", 1988500E24);
		CelestialBody jupiter = new CelestialBody("Jupiter", 1.8981E27);
		
		// Build the LEO parking orbit
		OrbitBuilder builder = new OrbitBuilder(
		OrbitDefinitionType.SCALAR_ELEMENT);
		builder.setCentralBody(earth);
		builder.setSMA(6378.0 * 1.1);
		builder.setEcc(0);
		builder.setInc(new Angle(0));
		builder.setRightAscOfAscNode(new Angle(0));
		builder.setArgOfPeri(new Angle(0));
		Orbit parkingOrbit = builder.build();
		
		// Build earth's orbit around the sun
		OrbitBuilder build = new OrbitBuilder(
		OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(1.0 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit earthsOrbit = build.build();

		// Build Jupiter's Orbit around the Sun
		build.setCentralBody(sun);
		build.setSMA(5.2 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit jupitersOrbit = build.build();
		
		// Definition of assorted constants
		final double JUP_RAD = jupitersOrbit.radiusAt(new Angle(Math.PI));
		final double JUP_VEL = jupitersOrbit.velocityAt(new Angle(Math.PI));
		final Vector3D JUP_RAD_VEC = jupitersOrbit.radiusVectorAt(new Angle(Math.PI));
		final Vector3D JUP_VEL_VEC = jupitersOrbit.velocityVectorAt(new Angle(Math.PI));
		final double hohmVa = Math.sqrt(sun.gravParam() * (2.0 / JUP_RAD - 1.0 / (3.1 * AU)));
		final double hohmVp = Math.sqrt(sun.gravParam() * (2.0 / AU - 1.0 / (3.1 * AU)));
		
		// Calculate the required heliocentric velocity magnitude after the Jupiter flyby
		double finalPeriodYears = 10.0;
		double finalIncDegrees;
		finalIncDegrees = 90.0;
//		finalIncDegrees = 80.0;
		Angle inc = new Angle(finalIncDegrees, AngleType.DEGREES);
		double finalSMA = Math.cbrt(sun.gravParam() * Math.pow( (finalPeriodYears / YEAR_S) / (Math.PI * 2.0), 2.0 ) );
		double finalHelioVel = Math.sqrt(sun.gravParam() * (2.0 / JUP_RAD - 1.0 / finalSMA));
		
		// Calculate the required v infinity after the Jupiter flyby
		double vInfOutMag = Math.sqrt(JUP_VEL * JUP_VEL + finalHelioVel * finalHelioVel - 2 * JUP_VEL * finalHelioVel * inc.cos());
		Angle vInfvJupAngle = new Angle((finalHelioVel * inc.sin()) / vInfOutMag, TrigFunction.SINE);
		
		// Energy of the second half of the jupiter flyby
		double hypOutEnergy = 0.5 * vInfOutMag * vInfOutMag;
		
		// Choose an rp, and calculte the properties of the second have of the jupiter flyby based on it
		double rp = 5 * R_JUP;
		double vpOut = Math.sqrt(2.0 * (hypOutEnergy + jupiter.gravParam() / rp));
		double sinDeltaBy2Out = 1.0 / (1.0 + (rp * vInfOutMag * vInfOutMag) / jupiter.gravParam());
		Angle deltaBy2Out = new Angle(sinDeltaBy2Out, TrigFunction.SINE);
		
		// Choose a vInfIn, and calculate the associated properties with it
		double vInfInMag;
//		vInfInMag = 8.28;
//		vInfInMag = 6.28;
		vInfInMag = 3.05;
		double energyIn = 0.5 * vInfInMag * vInfInMag;
		double vpIn = Math.sqrt(2.0 * (energyIn + jupiter.gravParam() / rp));
		double deltaVMag = vpOut - vpIn;
		double sinDeltaBy2In = 1.0 / (1.0 + (rp * vInfInMag * vInfInMag) / jupiter.gravParam());
		Angle deltaBy2In = new Angle(sinDeltaBy2In, TrigFunction.SINE);
		
		// Calculate some composite values
		Angle totalTurn = deltaBy2Out.add(deltaBy2In);
		Angle vInfInToVJup = new Angle(Math.PI).add(vInfvJupAngle.scalarMultiply(-1.0)).add(totalTurn.scalarMultiply(-1.0));
		
		// Calculate the properties of the arrival transfer
		Vector3D vInfInVec = (Vector3D) new Vector3D(0, vInfInToVJup.cos(), 1.0 * vInfInToVJup.sin()).scalarMultiply(vInfInMag);
		Vector3D scApoVel = (Vector3D) JUP_VEL_VEC.add(vInfInVec);
		
		Orbit arrivalOrbit = new Orbit(sun, JUP_RAD_VEC, scApoVel);
		
		
		// Calculate details of the earth departure orbit
		Vector3D transVa = (Vector3D) scApoVel.direction().scalarMultiply(hohmVa);
		Orbit transferOrbit = new Orbit(sun, JUP_RAD_VEC, transVa);
		Vector3D vpTrans = transferOrbit.velocityVectorAt(new Angle(0));
		Vector3D earthVInf = (Vector3D) vpTrans.add(earthsOrbit.velocityVectorAt(new Angle(0)).scalarMultiply(-1.0));
		double earthEnergy = 0.5 * earthVInf.magnitude() * earthVInf.magnitude();
		double earthVp = Math.sqrt(2.0 * (earthEnergy + earth.gravParam() / parkingOrbit.radiusAt(new Angle (0))));
		double earthVInfMag = earthVInf.magnitude();
		double earthVMag = earthsOrbit.velocityAt(new Angle(0));
		double cosEarthInc = ((hohmVp * hohmVp) - (earthVMag * earthVMag) - (earthVInfMag * earthVInfMag)) / (-2.0 * earthVMag * earthVInfMag);
		Angle earthInc = new Angle(cosEarthInc, TrigFunction.COSINE);
		
		if (earthInc.measure() > (Math.PI / 2)) {
			earthInc = new Angle(Math.PI).add(earthInc.scalarMultiply(-1.0));
		}
		
		double sinEarthDeltaBy2 = 1.0 / (1.0 + (parkingOrbit.radiusAt(new Angle(0)) * earthVInf.magnitude() * earthVInf.magnitude()) / earth.gravParam());
		Angle earthDeltaBy2 = new Angle(sinEarthDeltaBy2, TrigFunction.SINE);
		
		double earthDeltaV = earthVp - parkingOrbit.velocityAt(new Angle(0));
		
		double deltaVTotal = (scApoVel.magnitude() - hohmVa) + deltaVMag + earthDeltaV;
		
		
		System.out.println("TRANSFER PERI VEL: " + vpTrans + " KM/S");
		System.out.println("EARTH V_INF: " + earthVInf.magnitude() + " KM/S");
		System.out.println("EARTH VEL: " + earthsOrbit.velocityVectorAt(new Angle(0)));
		System.out.println("PARKING ORBIT INC: " + earthInc.measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("BURN BEFORE SUN-EARTH LINE: " + earthDeltaBy2.measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("EARTH DELTA V: " + earthDeltaV + " KM/S");
		
		System.out.println();
		System.out.println("V_INF OUT MAG: " + vInfOutMag + " KM/S");
		System.out.println("PRE-PERIAPSIS TURN ANGLE: " + deltaBy2In.measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("POST-PERIAPSIS TURN ANGLE: " + deltaBy2Out.measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("V_INF IN: " + vInfInVec + " KM/S");
		System.out.println("APOAPSIS VEL: " + scApoVel + " KM/S");
		System.out.println("MAGNITUDE: " + scApoVel.magnitude() + " KM/S");
		System.out.println("PERIAPSIS DELTA V: " + deltaVMag + " KM/S");
		System.out.println("TRANSFER ORBIT INCLINATION: " + arrivalOrbit.getInc().measure(AngleType.DEGREES) + " DEGREES");
		System.out.println("PRE-FLYBY DELTA V: " + (scApoVel.magnitude() - hohmVa) + " KM/S");
		
		System.out.println();
		System.out.println("MISSION DELTA V: " + deltaVTotal + " KM/S");
		
		double standardDeltaV = standardDeltaV(finalSMA, JUP_RAD / finalSMA - 1, inc);
		double percentSavings = ((standardDeltaV - deltaVTotal) / standardDeltaV) * 100;
		
		double isp = 340;
		double g0 = 9.81;
		double scMass = 250;
		double massFlyby = scMass * Math.exp((deltaVTotal * 1000) / (g0 * isp));
		double massStandard = scMass * Math.exp((standardDeltaV * 1000) / (g0 * isp));
		double massSavingsPercent = ((massStandard - massFlyby) / massStandard) * 100;
		
		System.out.println();
		System.out.println("TRADITONAL DELTA V: " + standardDeltaV + " KM/S");
		System.out.println("SAVINGS: " + decForm.format(percentSavings) + "%");
		System.out.println();
		System.out.println("FLYBY S/C MASS REQUIRED: " + massFlyby + " KG");
		System.out.println("STANDARD S/C MASS REQUIRED: " + massStandard + " KG");
		System.out.println("SAVINGS: " + massSavingsPercent + "%");
		
		
	}
	
	
	public static Vector3D velAfterFlybyMatrix(CelestialBody body, Vector3D bodyVel, double rp, Angle phi, Vector3D scVel) {
		Vector3D vInfIn = (Vector3D) scVel.add(bodyVel.scalarMultiply(-1.0));
		Angle theta = vInfIn.angleTo(bodyVel.direction());

		Matrix phiRot = Matrix.euler3D(phi.scalarMultiply(-1.0), 2);
		Matrix thetaRot = Matrix.euler3D(theta, 1);
		Matrix coordTrans = phiRot.multiply(thetaRot);
		Matrix inverseTrans = coordTrans.inverse();
		Matrix transformed = coordTrans.multiply(vInfIn.columnMatrix());

		double sinDeltaBy2 = 1.0 / (1.0 + (rp * vInfIn.magnitude() * vInfIn
				.magnitude()) / body.gravParam());
		Angle delta = new Angle(sinDeltaBy2, TrigFunction.SINE)
				.scalarMultiply(2.0);

		Matrix deltaRot = Matrix.euler3D(delta.scalarMultiply(-1.0), 1);
		Matrix vOutMatTrans = deltaRot.multiply(transformed);
		Matrix vOutMat = inverseTrans.multiply(vOutMatTrans);
		Vector3D vInfOut2 = new Vector3D(vOutMat);
		Vector3D vFinal = (Vector3D) bodyVel.add(vInfOut2);

		return vFinal;
	}

	
	public static Vector3D velAfterFlybyVec(CelestialBody body, Vector3D bodyVel, double rp, boolean posDelta, Vector3D scVel, double progradeDeltaV) {
		Vector3D vInfIn = (Vector3D) scVel.add(bodyVel.scalarMultiply(-1.0));
		Vector3D pIHat = (Vector3D) vInfIn.direction();
		Vector3D pJHat = new Vector3D(1, 0, 0);
		Vector3D pKHat = pIHat.cross(pJHat);
		Vector3D vInfInP = new Vector3D(vInfIn.magnitude(), 0, 0);
		
		double sinDeltaBy2 = 1.0 / (1.0 + (rp * vInfIn.magnitude() * vInfIn
				.magnitude()) / body.gravParam());
		Angle delta = new Angle(sinDeltaBy2, TrigFunction.SINE)
				.scalarMultiply(2.0);
		
//		System.out.println(pIHat);
		// I don't know about this
		if (!posDelta) {
			delta = delta.scalarMultiply(-1.0);
		}
		Matrix deltaRotMat = Matrix.euler3D(delta, 2);
		Matrix deltaBy2RotMat = Matrix.euler3D(delta.scalarMultiply(0.5), 2);
		
		double energyIn = 0.5 * vInfIn.magnitude() * vInfIn.magnitude();
		double smaIn = body.gravParam() / (-2.0 * energyIn);
		double periVel0Mag = Math.sqrt(body.gravParam() * (2.0 / rp - 1.0 / smaIn));
		
		Vector3D periVel0Dir = (Vector3D) new Vector3D(deltaBy2RotMat.multiply(vInfInP.columnMatrix())).direction();
		Vector3D periVel0 = (Vector3D) periVel0Dir.scalarMultiply(periVel0Mag);
		
		// -----------------------------------------------------------------------------------------
		Vector3D deltaV = (Vector3D) periVel0Dir.scalarMultiply(progradeDeltaV).add(new Vector3D(0, 0, 0));
//		Vector3D deltaV = new Vector3D(0, 0, 0);
		// -----------------------------------------------------------------------------------------
		
		Vector3D periVel1 = (Vector3D) periVel0.add(deltaV);
		
		double finalEnergy = 0.5 * periVel1.magnitude() * periVel1.magnitude() - body.gravParam() / rp;
		double finalSMA = body.gravParam() / (-2.0 * finalEnergy);
		double finalEcc = rp / finalSMA - 1.0;
		double finalVInf = Math.sqrt(2.0 * finalEnergy);
		
		Angle newDeltaBy2 = new Angle(1.0 / finalEcc, TrigFunction.SINE).scalarMultiply(-1.0);
		Matrix secondRotMat = Matrix.euler3D(newDeltaBy2, 2);
		
//		Vector3D periVelDir = (Vector3D) periVel1.direction();
		Matrix vInfOutDir = secondRotMat.multiply(periVel0Dir.columnMatrix());
		Matrix vInfOutMat = vInfOutDir.scalarMultiply(finalVInf);
		
		
		Vector3D vInfOut = (Vector3D) pIHat.scalarMultiply(vInfOutMat.getElement(1, 1)).add(
				pJHat.scalarMultiply(vInfOutMat.getElement(2, 1))).add(
				pKHat.scalarMultiply(vInfOutMat.getElement(3,1)));
		
		Vector3D vFinal = (Vector3D) bodyVel.add(vInfOut);

		return vFinal;
	}

	
	public static double standardDeltaV(double sma, double ecc, Angle inc) {
		// Constants
		final double AU = 149597871.0; // km in an AU
		final double YEAR_S = 1.0 / (60 * 60 * 24 * 365); // years in a second
		final double R_JUP = 71370.0;
		DecimalFormat decForm = new DecimalFormat("#.000");
		
		// Establish Celestial Bodies
		CelestialBody earth = new CelestialBody("Earth", 5.9726E24);
		CelestialBody sun = new CelestialBody("Sun", 1988500E24);
		CelestialBody jupiter = new CelestialBody("Jupiter", 1.8981E27);
		
		// Build the LEO parking orbit
		OrbitBuilder builder = new OrbitBuilder(
		OrbitDefinitionType.SCALAR_ELEMENT);
		builder.setCentralBody(earth);
		builder.setSMA(6378.0 * 1.1);
		builder.setEcc(0);
		builder.setInc(new Angle(0));
		builder.setRightAscOfAscNode(new Angle(0));
		builder.setArgOfPeri(new Angle(0));
		Orbit parkingOrbit = builder.build();
		
		// Build earth's orbit around the sun
		OrbitBuilder build = new OrbitBuilder(
		OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(1.0 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit earthsOrbit = build.build();
		
		// Build Jupiter's Orbit around the Sun
		build.setCentralBody(sun);
		build.setSMA(5.2 * AU);
		build.setEcc(0);
		build.setInc(new Angle(0));
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit jupitersOrbit = build.build();
				
		// Definition of assorted constants
		final double JUP_RAD = jupitersOrbit.radiusAt(new Angle(Math.PI));
		final double JUP_VEL = jupitersOrbit.velocityAt(new Angle(Math.PI));
		final Vector3D JUP_RAD_VEC = jupitersOrbit.radiusVectorAt(new Angle(Math.PI));
		final Vector3D JUP_VEL_VEC = jupitersOrbit.velocityVectorAt(new Angle(Math.PI));
		
		// Build the Earth - Jupiter heliocentric Hohmann transfer orbit
		OrbitPosition helioHohmStart = new OrbitPosition(earthsOrbit,
				new Angle(0));
		OrbitPosition helioHohmEnd = new OrbitPosition(jupitersOrbit,
				new Angle(Math.PI));
		LambertSolver solver = new LambertSolver(helioHohmStart, helioHohmEnd);
		LambertSolution solution = solver.solution(solver.aMin(), true);
		Orbit helioHohm = solution.orbitalPath();
		Vector3D hohmVp = helioHohm.velocityVectorAt(new Angle(0));
		Vector3D hohmVa = helioHohm.velocityVectorAt(new Angle(Math.PI));
		
		// Calculate the hyperbolic orbit around the Earth that will result in this transfer
		Vector3D earthVelAtStart = earthsOrbit.velocityVectorAt(new Angle(0));
		Vector3D earthVInfVec = (Vector3D) hohmVp.add(earthVelAtStart
				.scalarMultiply(-1.0));
		double earthVInf = earthVInfVec.magnitude();
		double parkingRadius = parkingOrbit.radiusAt(new Angle(0));
		double parkingVel = parkingOrbit.velocityAt(new Angle(0));
		double sinDeltaBy2Earth = 1.0 / (1.0 + (parkingRadius * earthVInf * earthVInf)
				/ earth.gravParam());
		Angle deltaEarth = new Angle(sinDeltaBy2Earth, TrigFunction.SINE)
				.scalarMultiply(2.0);
		double earthEnergy = 0.5 * earthVInf * earthVInf;
		double earthPeriVel = Math.sqrt(2 * (earthEnergy + earth.gravParam()
				/ parkingRadius));
		double earthDeltaV = earthPeriVel - parkingVel;
		
		// Compute the final orbit
		build = new OrbitBuilder(OrbitDefinitionType.SCALAR_ELEMENT);
		build.setCentralBody(sun);
		build.setSMA(sma);
		build.setEcc(ecc);
		build.setInc(inc);
		build.setRightAscOfAscNode(new Angle(0));
		build.setArgOfPeri(new Angle(0));
		Orbit finalOrbit = build.build();
		
		Vector3D apoVel = finalOrbit.velocityVectorAt(new Angle(Math.PI));
		Vector3D deltaV2Vec = (Vector3D) apoVel.add(hohmVa.scalarMultiply(-1.0));
		
		return earthDeltaV + deltaV2Vec.magnitude();
	}
}
